T1 = [    -0.935503,    -0.239730,     0.259544,   600.462224 ;
      0.204503,     0.231636,     0.951064,  -352.312158 ;
     -0.288119,     0.942801,    -0.167671,   812.144660 ;
      0.000000,     0.000000,     0.000000,     1.000000 ];
T2 = [     0.508774,    -0.331878,     0.794359,   407.662680 ;
     -0.494488,     0.642659,     0.585210,  -219.266469 ;
     -0.704720,    -0.690541,     0.162858,   687.752829 ;
      0.000000,     0.000000,     0.000000,     1.000000 ];
T3 = [     0.062069,     0.973213,     0.221368,   618.627290 ;
      0.242084,     0.200493,    -0.949315,   353.448822 ;
     -0.968268,     0.112513,    -0.223155,   830.025287 ;
      0.000000,     0.000000,     0.000000,     1.000000 ];
T4 = [     0.100496,     0.902367,     0.419088,   545.367780 ;
      0.986182,    -0.034584,    -0.162016,    64.101424 ;
     -0.131704,     0.429579,    -0.893373,  1082.223813 ;
      0.000000,     0.000000,     0.000000,     1.000000 ];
[R1,P1] = retreative_RP(T1);
[R2,P2] = retreative_RP(T2);
[R3,P3] = retreative_RP(T3);
[R4,P4] = retreative_RP(T4);
R = [R1;R2;R3;R4];
P = [P1;P2;P3;P4];
Pet = calcuateTet(R,P)      % tool frame in end frame coordinate
